#pragma once
namespace graph
{
   namespace algorithm
   {
      //yep it's very big function, but we can't partly specialize class methods to reuse code
      //8 variants (2*2*2) would give a lot of duplication and 8 methods to write
      template<mode_e mode, directional_e directional>
      inline void astar_initializer_t::init_me(astar_search_t<mode, directional>& as)
      {
         if (mode == debug)
            as.debug_info_ = alg_debug_info_ptr_t(new alg_debug_info_t());
            
         if (initializer_mode_ == details::particular_vertices)
         {
            as.start_vertex_id_ = start_vertex_id_;
            as.finish_vertex_id_ = finish_vertex_id_;
            vertex_t const & start_v = as.get_vertex_by_id(as.start_vertex_id_);
            vertex_t const & finish_v = as.get_vertex_by_id(as.finish_vertex_id_);
            as.start_p_ = start_v.get_point();
            as.finish_p_ = finish_v.get_point();
            
            if (directional == undirectional)
            {
               lat_lon_t initial_potential = math::distance(as.start_p_, as.finish_p_);//as.get_potential<forward>(as.start_p_);
               as.new_vertex_fwd(as.start_vertex_id_, initial_potential, as.start_vertex_id_);
               as.current_v_id_fwd_ = as.start_vertex_id_;
            }
            else if (directional == bidirectional)
            {
               //as.new_vertex_fwd(as.start_vertex_id_, as.get_potential<forward>(as.start_p_), as.start_vertex_id_);
               //as.new_vertex_bkwd(as.finish_vertex_id_, as.get_potential<backward>(as.finish_p_), as.finish_vertex_id_);
               as.new_vertex_fwd(as.start_vertex_id_, as.potential_fwd(as.start_p_), as.start_vertex_id_);
               as.new_vertex_bkwd(as.finish_vertex_id_, as.potential_bkwd(as.finish_p_), as.finish_vertex_id_);

               as.current_v_id_fwd_ = as.start_vertex_id_;               
               as.current_v_id_bkwd_ = as.finish_vertex_id_;
               as.min_path_len_of_bidirect_search_ = MAX_FLOAT;
            }
         }
         else if (initializer_mode_ == details::nearest_edges)
         {
            //lat_lon_t delta = 0.0001f;
            as.min_path_len_of_bidirect_search_ = MAX_FLOAT;
            if (directional == undirectional)
               throw graph_exception_t("undirectional search can't run with nearest_edges mode");
            as.start_vertex_id_ = START_VERTEX_ID;
            as.finish_vertex_id_ = FINISH_VERTEX_ID;
            as.finish_p_ = finish_p_;
            as.start_p_ = start_p_;            
            graph::global_data_t& global = graph::global::get_instance();
            //adding nearest vertices
            std::vector<vertex_id_t> nearest_fwd_vertices;
            std::vector<vertex_id_t> nearest_bkwd_vertices;
            //global.current_view_->get_nearest_vertices(as.start_p_, nearest_fwd_vertices);
            //global.current_view_->get_nearest_vertices(as.finish_p_, nearest_bkwd_vertices);
            //std::vector<vertex_id_t>::iterator v_it = nearest_fwd_vertices.begin();
            //std::vector<vertex_id_t>::iterator v_it_end = nearest_fwd_vertices.end();
            //for(; v_it != v_it_end; ++v_it)
            //{
            //   vertex_t const & v = (*as.view_)[*v_it];
            //   as.new_vertex_fwd(v.get_id(), 
            //                     math::manhattan_distance(as.start_p_, v.get_point()), 
            //                     as.start_vertex_id_ );
            //}
            //v_it = nearest_bkwd_vertices.begin();
            //v_it_end = nearest_bkwd_vertices.end();
            //for(; v_it != v_it_end; ++v_it)
            //{
            //   vertex_t const & v = (*as.view_)[*v_it];
            //   as.new_vertex_bkwd(v.get_id(), 
            //                      math::manhattan_distance(as.finish_p_, v.get_point()), 
            //                      as.finish_vertex_id_ );
            //}

            //adding nearest edges for forward direction
            std::vector<edge_t> nearest_fwd_edges;

            global.current_view_->get_nearest_edges(as.start_p_, nearest_fwd_edges);
            if (nearest_fwd_vertices.empty() && nearest_fwd_edges.empty())
            {
               error_ = NO_VERTICES_NEAR_GIVEN_POINTS_ERROR; 
               return;
            }

            
            std::vector<edge_t>::iterator it = nearest_fwd_edges.begin();
            std::vector<edge_t>::iterator end = nearest_fwd_edges.end();
            for(; it != end; ++it)
            {
               vertex_t& source_v = (*as.view_)[it->get_source_vertex_id()];
               vertex_t& target_v = (*as.view_)[it->get_target_vertex_id()];
               point_t nearest_p;
               lat_lon_t t;
               lat_lon_t min_dist = math::nearest_point(as.start_p_, source_v.get_point(), 
                                                        target_v.get_point(), nearest_p, t);
               
               if (t <= 0)
               {
                  min_dist = math::distance(as.start_p_, source_v.get_point()) * details::DIST_FINE;       
                  min_dist += as.potential_fwd(source_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_fwd_.has_key(source_v.get_id()))
                  {
                     if (as.vertex_data_fwd_.get_value(source_v.get_id()).distance_ > min_dist)
                        as.update_vertex_fwd(source_v.get_id(), min_dist, as.start_vertex_id_);
                  }
                  else
                     as.new_vertex_fwd(source_v.get_id(), min_dist, as.start_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_fwd_.push_back(line_t(as.start_p_, source_v.get_point()));
               }
               else if (t >= 1)
               {
                  min_dist = math::distance(as.start_p_, target_v.get_point()) * details::DIST_FINE;       
                  min_dist += as.potential_fwd(target_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_fwd_.has_key(target_v.get_id()))
                  {
                     if (as.vertex_data_fwd_.get_value(target_v.get_id()).distance_ > min_dist)
                        as.update_vertex_fwd(target_v.get_id(), min_dist, as.start_vertex_id_);
                  }
                  else
                     as.new_vertex_fwd(target_v.get_id(), min_dist, as.start_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_fwd_.push_back(line_t(as.start_p_, target_v.get_point()));

               }
               else //t > 0 && t < 1
               {
                  as.nearest_start_ps_.set_value(target_v.get_id(), nearest_p);
                  min_dist = math::distance(as.start_p_, nearest_p) * details::DIST_FINE;
                  min_dist += math::distance(nearest_p, target_v.get_point());
                  min_dist += as.potential_fwd(target_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_fwd_.has_key(target_v.get_id()))
                  {
                     if ( as.vertex_data_fwd_.get_value(target_v.get_id()).distance_ > min_dist)
                        as.update_vertex_fwd(target_v.get_id(), min_dist, as.start_vertex_id_);
                  }
                  else
                     as.new_vertex_fwd(target_v.get_id(), min_dist, as.start_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_fwd_.push_back(line_t(as.start_p_, nearest_p));

               }
            }
            
            //adding nerest edges for backward direction
            std::vector<edge_t> nearest_bkwd_edges;
            global.current_view_->get_nearest_edges(as.finish_p_, nearest_bkwd_edges);
            if ((nearest_bkwd_vertices.empty() && nearest_bkwd_edges.empty()))
            {
               error_ = NO_VERTICES_NEAR_GIVEN_POINTS_ERROR;
               return;
            }

            it = nearest_bkwd_edges.begin();
            end = nearest_bkwd_edges.end();
            for(; it != end; ++it)
            {
               vertex_t& source_v = (*as.view_)[it->get_source_vertex_id()];
               vertex_t& target_v = (*as.view_)[it->get_target_vertex_id()];
               point_t nearest_p;
               lat_lon_t t;
               lat_lon_t min_dist = math::nearest_point(as.finish_p_, source_v.get_point(), 
                                                        target_v.get_point(), nearest_p, t);
               
               if (t <= 0)
               {
                  min_dist = math::distance(as.finish_p_, source_v.get_point()) * details::DIST_FINE;       
                  min_dist += as.potential_bkwd(source_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_bkwd_.has_key(source_v.get_id()))
                  {
                     if ( as.vertex_data_bkwd_.get_value(source_v.get_id()).distance_ > min_dist)     
                        as.update_vertex_bkwd(source_v.get_id(), min_dist, as.finish_vertex_id_);
                  }
                  else
                     as.new_vertex_bkwd(source_v.get_id(), min_dist, as.finish_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_bkwd_.push_back(line_t(as.finish_p_, source_v.get_point()));
               }
               else if (t >= 1)
               {
                  min_dist = math::distance(as.finish_p_, target_v.get_point()) * details::DIST_FINE;       
                  min_dist += as.potential_bkwd(target_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_bkwd_.has_key(target_v.get_id()))
                  {
                     if (as.vertex_data_bkwd_.get_value(target_v.get_id()).distance_ > min_dist)
                        as.update_vertex_bkwd(target_v.get_id(), min_dist, as.finish_vertex_id_);
                  }
                  else
                     as.new_vertex_bkwd(target_v.get_id(), min_dist, as.finish_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_bkwd_.push_back(line_t(as.finish_p_, target_v.get_point()));

               }
               else //t > 0 && t < 1
               {
                  as.nearest_finish_ps_.set_value(target_v.get_id(), nearest_p);

                  min_dist = math::distance(as.finish_p_, nearest_p) * details::DIST_FINE;
                  min_dist += math::distance(nearest_p, target_v.get_point());
                  min_dist += as.potential_bkwd(target_v.get_point());//as.get_potential<forward>(source_v.get_point()),                        
                  if (as.vertex_data_bkwd_.has_key(target_v.get_id()))
                  {
                     if (as.vertex_data_bkwd_.get_value(target_v.get_id()).distance_ > min_dist)
                        as.update_vertex_bkwd(target_v.get_id(), min_dist, as.finish_vertex_id_);
                  }
                  else
                     as.new_vertex_bkwd(target_v.get_id(), min_dist, as.finish_vertex_id_);
                  if (mode == debug)
                     as.debug_info_->output_list_bkwd_.push_back(line_t(as.finish_p_, nearest_p));
               }
            }
         }
      }
      
      inline int astar_initializer_t::get_last_error()
      {
         return error_;
      }
   }
}
